#include "Trace.h"
#include "Random.h"

#include <omp.h>

//////////////////////////////////////////////////////////////////////////
/// mpGenerateEyeRays
//////////////////////////////////////////////////////////////////////////

void mpGenerateEyeRays(IN  C_Camera	& camera,
											 IN  int2			& resolution,
											 IN  int2			& sample,
											 IN  uint32			type, 
											 OUT float4		* pRayO,
											 OUT float4		* pRayD )
{
	CBenchmark genraysBench;

	float3 eye = camera.Eye();
	float3 dir = camera.Dir();
	float3 up  = camera.Up();
	
	float  zn  = camera.zN();
	float  zf  = camera.zF();
	float  asp = camera.Asp();
	float  fov = camera.FoV();

	float3 right = camera.Right();

	// virtual plane
	float2 vp; 
	vp.x = zn * tanf(to_radian(fov * 0.5f));
	vp.y = vp.x / asp;

	float2 scale = { 1.0f, 1.0f };

	if (type == RT_PERSPECTIVE)
	{
		genraysBench.Start("mpGenerateEyeRays Perspective");

		scale.x = scale.x / (resolution.x * 0.5f) * vp.x;
		scale.y = scale.y / (resolution.y * 0.5f) * vp.y;

#ifdef _OPENMP
#pragma omp parallel for
#endif
		for (int32 ry = 0; ry < resolution.y; ry++)
			for (int32 rx = 0; rx < resolution.x; rx++)
			{
				float3 Eye;
				float3 Dir;

				for (int32 sy = 0; sy < sample.y; sy++)
					for (int32 sx = 0; sx < sample.x; sx++)
					{
						// sampling plane
						float2 sp = { (sx + 0.5f) / sample.x , (sy + 0.5f) / sample.y };

						float2 pp;
						pp.x = scale.x * (rx - 0.5f * resolution.x + sp.x);
						pp.y = scale.y * (ry - 0.5f * resolution.y + sp.y);

						Dir = normalize(right * pp.x + up * pp.y + dir * zn);

						Eye = eye + Dir;

						uint32 iRay = (sx + rx * sample.x) + (sy + ry * sample.y) * (resolution.x * sample.x);
						pRayD[iRay] = f4(Dir);
						pRayO[iRay] = f4(Eye);
					}
			}	
		genraysBench.End();
	}

	if (type == RT_ORTHOGONAL)
	{
		genraysBench.Start("mpGenerateEyeRays Orthogonal");

		float3 Dir = dir;

#ifdef _OPENMP
#pragma omp parallel for
#endif
		for (int32 ry = 0; ry < resolution.y; ry++)
			for (int32 rx = 0; rx < resolution.x; rx++)
			{
				for (int32 sy = 0; sy < sample.y; sy++)
					for (int32 sx = 0; sx < sample.x; sx++)
					{
						// sampling plane
						float2 sp = { (sx + 0.5f) / sample.x , (sy + 0.5f) / sample.y };

						float2 pp;
						pp.x = scale.x * (rx - 0.5f * resolution.x + sp.x);
						pp.y = scale.y * (ry - 0.5f * resolution.y + sp.y);

						float3 Eye = eye + right * pp.x + up * pp.y;

						int32 iRay = (sx + rx * sample.x) + (sy + ry * sample.y) * (resolution.x * sample.x);
						pRayD[iRay] = f4(Dir);
						pRayO[iRay] = f4(Eye);
					}
			}
		genraysBench.End();
	}
}

template<class T>
void mpResolveBuffer_v4(IN  int2     & resolution,
												IN  int2     & sample,
												IN  uint32     filter,	
												OUT T        * pIn,
												OUT T        * pOut)
{
	CBenchmark resolveBench;

	resolveBench.Start("mpGenerateEyeRays Perspective");

#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (int32 ry = 0; ry < resolution.y; ry++)
		for (int32 rx = 0; rx < resolution.x; rx++)
		{
			float4 out = f4();

			for (int32 sy = 0; sy < sample.y; sy++)
				for (int32 sx = 0; sx < sample.x; sx++)
				{
					int32 idx = (sx + rx * sample.x) + (sy + ry * sample.y) * (resolution.x * sample.x);
					out += f4(pIn[idx]);
				}

			int32 idx = rx + ry * (resolution.x);
			out = out * (1.0f / (sample.x * sample.y));

			T result = { out.x, out.y, out.z, out.w };

			pOut[idx] = result;
		}	

		resolveBench.End();
}

template<class T>
void mpResolveBuffer_v3(IN  int2     & resolution,
												IN  int2     & sample,
												IN  uint32     filter,	
												OUT T        * pIn,
												OUT T        * pOut)
{
	CBenchmark resolveBench;

	resolveBench.Start("mpGenerateEyeRays Perspective");

#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (int32 ry = 0; ry < resolution.y; ry++)
		for (int32 rx = 0; rx < resolution.x; rx++)
		{
			float3 out = f3();

			for (int32 sy = 0; sy < sample.y; sy++)
				for (int32 sx = 0; sx < sample.x; sx++)
				{
					int32 idx = (sx + rx * sample.x) + (sy + ry * sample.y) * (resolution.x * sample.x);
					out += f3(pIn[idx]);
				}

				int32 idx = rx + ry * (resolution.x);
				out = out * (1.0f / (sample.x * sample.y));

				T result = { out.x, out.y, out.z };

				pOut[idx] = result;
		}	

		resolveBench.End();
}

template<class T>
void mpResolveBuffer_v2(IN  int2     & resolution,
												IN  int2     & sample,
												IN  uint32     filter,	
												OUT T        * pIn,
												OUT T        * pOut)
{
	CBenchmark resolveBench;

	resolveBench.Start("mpGenerateEyeRays Perspective");

#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (int32 ry = 0; ry < resolution.y; ry++)
		for (int32 rx = 0; rx < resolution.x; rx++)
		{
			float2 out = f2();

			for (int32 sy = 0; sy < sample.y; sy++)
				for (int32 sx = 0; sx < sample.x; sx++)
				{
					int32 idx = (sx + rx * sample.x) + (sy + ry * sample.y) * (resolution.x * sample.x);
					out += f2(pIn[idx]);
				}

				int32 idx = rx + ry * (resolution.x);
				out = out * (1.0f / (sample.x * sample.y));

				T result = { out.x, out.y };

				pOut[idx] = result;
		}	

		resolveBench.End();
}

template<class T>
void mpResolveBuffer_v1(IN  int2     & resolution,
												IN  int2     & sample,
												IN  uint32     filter,	
												OUT T        * pIn,
												OUT T        * pOut)
{
	CBenchmark resolveBench;

	resolveBench.Start("mpGenerateEyeRays Perspective");

#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (int32 ry = 0; ry < resolution.y; ry++)
		for (int32 rx = 0; rx < resolution.x; rx++)
		{
			float1 out = f1();

			for (int32 sy = 0; sy < sample.y; sy++)
				for (int32 sx = 0; sx < sample.x; sx++)
				{
					int32 idx = (sx + rx * sample.x) + (sy + ry * sample.y) * (resolution.x * sample.x);
					out += f1(pIn[idx]);
				}

				int32 idx = rx + ry * (resolution.x);
				out = out * (1.0f / (sample.x * sample.y));

				T result = { out.x };

				pOut[idx] = result;
		}	

		resolveBench.End();
}

void mpGenerateEyeRay(IN  C_Camera & camera,
											IN  int2    & resolution,
											IN  uint32    type, 
											IN  int       x,
											IN  int       y,
											OUT float3  & rayO,
											OUT float3  & rayD,
											OUT int     & iRay)
{
	CBenchmark genraysBench;

	float3 eye = camera.Eye();
	float3 dir = camera.Dir();
	float3 up  = camera.Up();
	
	float  zn  = camera.zN();
	float  zf  = camera.zF();
	float  asp = camera.Asp();
	float  fov = camera.FoV();

	float3 right = camera.Right();

	float2 s = { 1.0f, 1.0f };

	// virtual plane
	float2 vp; 
	vp.x = zn * tanf(to_radian(fov * 0.5f));
	vp.y = vp.x / asp;

	int32 ry = y;
	int32 rx = x;

	if (type == RT_PERSPECTIVE)
	{
		genraysBench.Start("mpGenerateEyeRays Perspective");

		s.x = s.x / (resolution.x * 0.5f) * vp.x;
		s.y = s.y / (resolution.y * 0.5f) * vp.y;

		float2 sp = {0.5f , 0.5f};

		float2 pp;
		pp.x = s.x * (rx - 0.5f * resolution.x + sp.x);
		pp.y = s.y * (ry - 0.5f * resolution.y + sp.y);
		rayD = right * pp.x + up * pp.y + dir * zn;

		rayD = normalize(rayD);

		rayO = eye + rayD;

		iRay = rx + ry * resolution.x;

		genraysBench.End();
	}

	if (type == RT_ORTHOGONAL)
	{
		genraysBench.Start("mpGenerateEyeRays Orthogonal");

		rayD = dir;

		float2 sp = {0.5f, 0.5f};

		float2 pp;
		pp.x = s.x * (rx - 0.5f * resolution.x + sp.x);
		pp.y = s.y * (ry - 0.5f * resolution.y + sp.y);

		rayO = eye + right * pp.x + up * pp.y;

		iRay = rx + ry * resolution.x;

		genraysBench.End();
	}
}

void mpTrace(IN  float4  * pRayO,
						 IN  float4  * pRayD,
						 IN  int32     nRay,
						 IN  CglMesh * pMesh,
						 IN  uint32    nMesh,
						 OUT float   * pTmin, 
						 OUT int2    * pId,
						 OUT uchar4  * pRgba8)
{
	CBenchmark traceBench;

	traceBench.Start("mpTrace");
#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (int32 iRay = 0; iRay < nRay; iRay++)
	{
		float3 rayO = xyz(pRayO[iRay]);
		float3 rayD = xyz(pRayD[iRay]);

		float tIntersect = CU_MAX_F32;

		int2 id = { -1, -1 };

		for (uint32 iMesh = 0; iMesh < nMesh; iMesh++)
		{
			uint32  nIdx    = pMesh[iMesh].posIdx.size();
			int32  *pPosIdx = pMesh[iMesh].posIdx.hPtrR() + pMesh[iMesh].posIdx.offsetR();
			float4 *pPos    = pMesh[iMesh].pos.hPtrW()    + pMesh[iMesh].pos.offsetR();

			for (uint32 iIdx = 0; iIdx < nIdx; iIdx+=3)
			{
				int3 face; 
				float3 v[3];

				face = i3( pPosIdx[iIdx+0], pPosIdx[iIdx+1], pPosIdx[iIdx+2] );
				v[0] = xyz(pPos[face.x]);
				v[1] = xyz(pPos[face.y]); 
				v[2] = xyz(pPos[face.z]);

				if (Intersect(v[0], v[1], v[2], rayO, rayD, tIntersect))
				{
					float tPrevIntersect = pTmin[iRay];
					if (tPrevIntersect > tIntersect)
					{
						uchar4 white = {255, 255, 255, 255};

						pRgba8[iRay] = white;
						pTmin[iRay] = tIntersect;					
						
						set(id, iMesh, iIdx );
						pId[iRay] = id;
					}
				}
			}
		}
	}
	traceBench.End();
}

void mpTraceKdTree(IN  CSpatialTree * pKd,
									 IN  S_Box       & box,
									 IN  float4       * pRayO,
									 IN  float4       * pRayD, 
									 IN  int32          nRay,
									 IN  float4       * pVertex,
									 IN  int32        * pFace,
									 IN  uint32       * pIndex,
									 OUT float        * pTmin, 
									 OUT int2         * pId, 
									 OUT uchar4       * pRgba8)
{
	CBenchmark traceBench;

	KdNode * pNodes = (KdNode *) pKd->pKdTree();

	traceBench.Start("mpTraceKdTree");

#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (int32 iRay = 0; iRay < nRay; iRay++)
	{
		KdNode node_stack[64];

		float tnear = 0.0f, tfar = 0.0f, split = 0.0f, tsplit = 0.0f, thit = CU_MAX_F32, tprev = pTmin[iRay];
		int stack_index = 0;
		int node_index = 0;
		int axis = TREE_LEAF;

		float3 rayD = xyz(pRayD[iRay]);
		float3 rayO = xyz(pRayO[iRay]);

		int2 id = { -1, -1 };

		bool bHit = true;

		float3 inv_dir = 1.0f / rayD;
		int    sign_dir[3] = { 0, 0, 0 };

		sign_dir[0] = (rayD.x >= 0) ? 1 : 0;
		sign_dir[1] = (rayD.y >= 0) ? 1 : 0;
		sign_dir[2] = (rayD.z >= 0) ? 1 : 0;

		if (Intersect(box.pmin, box.pmax, rayO, rayD, tnear, tfar))
		{
			if (tnear < 0) tnear = 0.0f;
			bHit = false;
		}

		while( !bHit )
		{
			split = pNodes[node_index].Split();   // as_float(as_int(pNodes[node_index].split) & (~KD_LEAF));
			axis  = pNodes[node_index].Axis();    // as_int(pNodes[node_index].split) & KD_LEAF;

			tsplit = (split - ((float*)&rayO)[axis]) * ((float*)&inv_dir)[axis];

			int first_left = sign_dir[axis];

			int near = 0;
			int far = 0;

			int left = node_index+1;
			int right = pNodes[node_index].next;

			if (axis != TREE_LEAF)
			{
				if (first_left)
				{
					near = left;
					far = right;
				}
				else
				{
					far = left;
					near = right;
				}

				if(tsplit < tnear)
				{
					node_index = far;
				}
				else if (tsplit > tfar)
				{
					node_index = near;
				}
				else
				{
					node_index = near;
					node_stack[stack_index].t = tfar;
					node_stack[stack_index].next = far;

					stack_index++;

					tfar = tsplit;
				}
			}
			else
			{
				int32 prim_start = pNodes[node_index].start;
				int32 prim_count = pNodes[node_index].Count();

				for (int32 iPrimitive = 0; iPrimitive < prim_count; iPrimitive++)
				{
					int32 face_idx = pIndex[ prim_start + iPrimitive ];

					int3 face = i3( pFace[3*face_idx], pFace[3*face_idx+1], pFace[3*face_idx+2] );

					float3 v[3] = { xyz(pVertex[face.x]), xyz(pVertex[face.y]), xyz(pVertex[face.z]) };

					if (Intersect(v[0], v[1], v[2], rayO, rayD, thit))
					{					
						if (tprev > thit)
						{
							const uchar4 white = {255, 255, 255, 255};

							pRgba8[iRay] = white;
							tprev = thit;					
							
							pId[iRay] = i2(face_idx, 0);

							bHit = true;
						} // (tprev > thit)
					}  // if (Intersect(v[0], v[1], v[2], rayO, rayD, thit))
				} // for (int32 iPrimitive = 0; iPrimitive < prim_count; iPrimitive++)

				if (!bHit && stack_index > 0) 
				{	
					stack_index--;
					
					tnear = tfar;
					tfar = node_stack[stack_index].t;
					node_index = node_stack[stack_index].next;
				}
				else
				{
					pTmin[iRay] = tprev;
					bHit = true;
				}
			} //if (axis != KD_LEAF)
		} // while (!bhit)
	} // rays

	traceBench.End();
}

void mpTraceKdTree(IN  CSpatialTree * pKd,
									 IN  S_Box       & box,
									 IN  float3       & orig,
									 IN  float3       & dir,
									 IN  int            iRay,
									 IN  float4       * pPosition,
									 IN  int32        * pFace,
									 IN  uint32       * pIndex,
									 OUT float        * pTmin, 
									 OUT int2         * pId, 
									 OUT uchar4       * pRgba8)
{
	CBenchmark traceBench;

	KdNode * pNodes = (KdNode *) pKd->pKdTree();

	traceBench.Start("mpTraceKdTree");

	pRgba8[iRay] = uc4(0, 0, 255, 255);

	KdNode node_stack[64];

	float tnear = 0.0f, tfar = 0.0f, split = 0.0f, tsplit = 0.0f, thit = CU_MAX_F32, tprev = thit;
	int stack_index = 0;
	int node_index = 0;
	int axis = TREE_LEAF;

	float3 rayD = dir;  //xyz(pRayD[iRay]);
	float3 rayO = orig; //xyz(pRayO[iRay]);

	int2 id = { -1, -1 };

	bool bHit = true;

	float3 inv_dir = 1.0f / rayD;
	int    sign_dir[3] = { 0, 0, 0 };

	sign_dir[0] = (rayD.x >= 0) ? 1 : 0;
	sign_dir[1] = (rayD.y >= 0) ? 1 : 0;
	sign_dir[2] = (rayD.z >= 0) ? 1 : 0;

	if (Intersect(box.pmin, box.pmax, rayO, rayD, tnear, tfar))
	{
		if (tnear < 0) tnear = 0.0f;
		bHit = false;
	}

	while( !bHit )
	{
		split = pNodes[node_index].Split();
		axis  = pNodes[node_index].Axis();

		tsplit = (split - ((float*)&rayO)[axis]) * ((float*)&inv_dir)[axis];

		int first_left = sign_dir[axis];

		int near = 0;
		int far = 0;

		int left = node_index+1;
		int right = pNodes[node_index].next;

		if (axis != TREE_LEAF)
		{
			if (first_left)
			{
				near = left;
				far = right;
			}
			else
			{
				far = left;
				near = right;
			}

			if(tsplit < tnear)
			{
				node_index = far;
			}
			else if (tsplit > tfar)
			{
				node_index = near;
			}
			else
			{
				node_index = near;
				node_stack[stack_index].t = tfar;
				node_stack[stack_index].next = far;

				stack_index++;

				tfar = tsplit;
			}
		}
		else
		{
			int32 prim_start = pNodes[node_index].start;
			int32 prim_count = pNodes[node_index].Count();

			for (int32 iPrimitive = 0; iPrimitive < prim_count; iPrimitive++)
			{
				int32 face_idx = pIndex[ prim_start + iPrimitive ];

				int3 face = i3( pFace[3*face_idx], pFace[3*face_idx+1], pFace[3*face_idx+2] );

				float3 v[3] = { xyz(pPosition[face.x]), xyz(pPosition[face.y]), xyz(pPosition[face.z]) };

				if (Intersect(v[0], v[1], v[2], rayO, rayD, thit))
				{					
					if (tprev > thit)
					{
						const uchar4 white = {255, 255, 255, 255};

						pRgba8[iRay] = white;
						tprev = thit;					
						
						pId[iRay] = i2(face_idx, 0);

						bHit = true;
					} // (tprev > thit)
				}  // if (Intersect(v[0], v[1], v[2], rayO, rayD, thit))
			} // for (int32 iPrimitive = 0; iPrimitive < prim_count; iPrimitive++)

			if (!bHit && stack_index > 0) 
			{
				stack_index--;

				tnear = tfar;
				tfar = node_stack[stack_index].t;
				node_index = node_stack[stack_index].next;
			}
			else
			{
				pTmin[iRay] = tprev;
				bHit = true;
			}
		} //if (axis != KD_LEAF)
	} // while (!bhit)

	traceBench.End();
}


void mpTraceBVHTree(IN  CSpatialTree * pBVH,  
										IN  float4       * pRayO, 
										IN  float4       * pRayD,
										IN  int32          nRay,
										IN  float4       * pPosition, 
										IN  int32        * pFace, 
										IN  uint32       * pIndex,
										OUT float        * pTmin, 
										OUT int2         * pId, 
										OUT uchar4       * pRgba8)
{
	CBenchmark traceBench;

	BVHNode * pNodes = (BVHNode *) pBVH->pBVHTree();

	traceBench.Start("mpTraceBVHTree");

#ifdef _OPENMP
#pragma omp parallel for
#endif
	for (int32 iRay = 0; iRay < nRay; iRay++)
	{
		int node_stack[64];

		float tnear = 0.0f, tfar = 0.0f, split = 0.0f, tsplit = 0.0f, thit = CU_MAX_F32, tprev = pTmin[iRay];
		int stack_index = 0;
		int node_index = TREE_ROOT;
		int near, far, first_left;

		float3 rayD = xyz(pRayD[iRay]);
		float3 rayO = xyz(pRayO[iRay]);

		int2 id = { -1, -1 };

		bool bHit = true;

		float3 inv_dir = 1.0f / rayD;
		int sign_dir[3] = { 0, 0, 0 }; 

		sign_dir[0] = (rayD.x >= 0) ? 1 : 0;
		sign_dir[1] = (rayD.y >= 0) ? 1 : 0;
		sign_dir[2] = (rayD.z >= 0) ? 1 : 0;

		BVHNode root = pNodes[node_index];

		float3 pmin = root.pmin;
		float3 pmax = root.pmax;

		if (Intersect(pmin, pmax, rayO, rayD, tnear, tfar))
		{
			if (tnear < 0) tnear = 0.0f;

			int axis = root.Axis();
			float split = root.Split();

			float o = ((float*)&rayO)[axis];
			float d = ((float*)&inv_dir)[axis];

			tsplit = (split - o) * d;

			first_left = sign_dir[axis];

			if (first_left) 
			{
				near = node_index+1;
				far = pNodes[node_index].right;
			}
			else
			{
				near = pNodes[node_index].right;
				far = node_index+1;
			}

			if(tsplit < tnear)
			{
				node_stack[stack_index++] = far;
			}
			else if (tsplit > tfar)
			{
				node_stack[stack_index++] = near;
			}
			else
			{
				node_stack[stack_index++] = far;
				node_stack[stack_index++] = near;
			}
			bHit = false;
		}

		while (!bHit && stack_index > 0)
		{
			stack_index--;

			node_index = node_stack[stack_index];
			BVHNode bvh = pNodes[node_index];

			pmin = bvh.pmin;
			pmax = bvh.pmax;

			if (Intersect(pmin, pmax, rayO, rayD, tnear, tfar))
			{
				if (tnear < 0) tnear = 0.0f;

				int axis = bvh.Axis();
				float split = bvh.Split();

				if (axis != TREE_LEAF)
				{
					float o = ((float*)&rayO)[axis];
					float d = ((float*)&inv_dir)[axis];

					tsplit = (split - o) * d;

					first_left = sign_dir[axis];

					if (first_left) 
					{
						near = node_index+1;
						far = pNodes[node_index].right;
					}
					else
					{
						near = pNodes[node_index].right;
						far = node_index+1;
					}

					if(tsplit < tnear)
					{
						node_stack[stack_index++] = far;
					}
					else if (tsplit > tfar)
					{
						node_stack[stack_index++] = near;
					}
					else
					{
						node_stack[stack_index++] = far;
						node_stack[stack_index++] = near;
					}
				} // if (axis != KD_LEAF)
				else
				{
					int32 prim_start = bvh.start;
					int32 prim_count = bvh.Count();

					for (int32 iPrimitive = 0; iPrimitive < prim_count; iPrimitive++)
					{
						int32 face_idx = pIndex[ prim_start + iPrimitive ];

						int3 face = i3( pFace[3*face_idx], pFace[3*face_idx+1], pFace[3*face_idx+2] );

						float3 v[3] = { xyz(pPosition[face.x]), xyz(pPosition[face.y]), xyz(pPosition[face.z]) };

						if (Intersect(v[0], v[1], v[2], rayO, rayD, thit))
						{					
							if (tprev > thit)
							{										
								tprev = thit;					
								
								pId[iRay] = i2(face_idx, 0);

								bHit = true;
							} // if (tprev > thit)
						} //if (Intersect(v[0], v[1], v[2], rayO, rayD, thit))
					} // for (int32 iPrimitive = 0; iPrimitive < prim_count; iPrimitive++)					
					
					if (bHit) 
					{
						const uchar4 white = {255, 255, 255, 255};	
						pRgba8[iRay] = white;
						pTmin[iRay] = tprev;
					} // if (bHit) 
				} // else (if (axis != KD_LEAF))
			} // if (Intersect(pmin, pmax, rayO, rayD, tnear, tfar))
		} // while (!bHit && stack_index > 0)
	} // rays

	traceBench.End();
}

void mpTraceBVHTree(IN  CSpatialTree * pBVH,  
										IN  float3       & orig,
										IN  float3       & dir,
										IN  int32          iRay,
										IN  float4       * pPosition, 
										IN  int32        * pFace, 
										IN  uint32       * pIndex,
										OUT float        * pTmin, 
										OUT int2         * pId, 
										OUT uchar4       * pRgba8)
{
	CBenchmark traceBench;

	pRgba8[iRay] = uc4(0, 0, 255, 255);

	BVHNode * pNodes = (BVHNode *) pBVH->pBVHTree();

	traceBench.Start("mpTraceBVHTree");

	int node_stack[64];

	float tnear = 0.0f, tfar = 0.0f, split = 0.0f, tsplit = 0.0f, thit = CU_MAX_F32, tprev = thit;
	int stack_index = 0;
	int node_index = TREE_ROOT;
	int near, far, first_left;

	float3 rayD = dir;  
	float3 rayO = orig; 

	int2 id = { -1, -1 };

	bool bHit = true;

	float3 inv_dir = 1.0f / rayD;
	int sign_dir[3] = { 0, 0, 0 }; 

	sign_dir[0] = (rayD.x >= 0) ? 1 : 0;
	sign_dir[1] = (rayD.y >= 0) ? 1 : 0;
	sign_dir[2] = (rayD.z >= 0) ? 1 : 0;

	BVHNode bvh = pNodes[node_index];

	float3 pmin = bvh.pmin;
	float3 pmax = bvh.pmax;

	if (Intersect(pmin, pmax, rayO, rayD, tnear, tfar))
	{
		if (tnear < 0) tnear = 0.0f;

		int axis = bvh.Axis();
		float split = bvh.Split();

		float o = ((float*)&rayO)[axis];
		float d = ((float*)&inv_dir)[axis];

		tsplit = (split - o) * d;

		first_left = sign_dir[axis];

		if (first_left) 
		{
			near = node_index+1;
			far = pNodes[node_index].right;
		}
		else
		{
			near = pNodes[node_index].right;
			far = node_index+1;
		}

		if(tsplit < tnear)
		{
			node_stack[stack_index++] = far;
		}
		else if (tsplit > tfar)
		{
			node_stack[stack_index++] = near;
		}
		else
		{
			node_stack[stack_index++] = far;
			node_stack[stack_index++] = near;
		}

		bHit = false;
	} // if (Intersect(pmin, pmax, rayO, rayD, tnear, tfar)) (ROOT NODE)

	while (!bHit && stack_index > 0)
	{
		stack_index--;

		node_index = node_stack[stack_index];
		bvh = pNodes[node_index];

		pmin = bvh.pmin;
		pmax = bvh.pmax;

		if (Intersect(pmin, pmax, rayO, rayD, tnear, tfar))
		{
			if (tnear < 0) tnear = 0.0f;

			int axis = bvh.Axis();
			float split = bvh.Split();

			if (axis != TREE_LEAF)
			{
				float o = ((float*)&rayO)[axis];
				float d = ((float*)&inv_dir)[axis];

				tsplit = (split - o) * d;

				first_left = sign_dir[axis];

				if (first_left) 
				{
					near = node_index+1;
					far = pNodes[node_index].right;
				}
				else
				{
					near = pNodes[node_index].right;
					far = node_index+1;
				}

				if(tsplit < tnear)
				{
					node_stack[stack_index++] = far;
				}
				else if (tsplit > tfar)
				{
					node_stack[stack_index++] = near;
				}
				else
				{
					node_stack[stack_index++] = far;
					node_stack[stack_index++] = near;
				}
			}
			else
			{
				int32 prim_start = bvh.start;
				int32 prim_count = bvh.Count();

				for (int32 iPrimitive = 0; iPrimitive < prim_count; iPrimitive++)
				{
					int32 face_idx = pIndex[ prim_start + iPrimitive ];

					int3 face = i3( pFace[3*face_idx], pFace[3*face_idx+1], pFace[3*face_idx+2] );

					float3 v[3] = { xyz(pPosition[face.x]), xyz(pPosition[face.y]), xyz(pPosition[face.z]) };

					if (Intersect(v[0], v[1], v[2], rayO, rayD, thit))
					{					
						if (tprev > thit)
						{										
							tprev = thit;					
							
							pId[iRay] = i2(face_idx, 0);

							bHit = true;
						} // if (tprev > thit)
					} // if (Intersect(v[0], v[1], v[2], rayO, rayD, thit))
				} // for (int32 iPrimitive = 0; iPrimitive < prim_count; iPrimitive++)					
				
				if (bHit) 
				{
					const uchar4 white = {255, 255, 255, 255};	
					pRgba8[iRay] = white;
					pTmin[iRay] = tprev;
				} // if (bHit)
			} // else (axis != KD_LEAF
		} // if (Intersect(pmin, pmax, rayO, rayD, tnear, tfar))
	} // while (!bHit && stack_index > 0)
	
	traceBench.End();
}